CON

  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000

  LR_PIN        = 2
  UD_PIN        = 3

  SW_UP           = 19
  SW_DN           = 17
  SW_LF           = 20
  SW_RT           = 18
  SW_CN           = 16

OBJ

  rc    :       "RCTIME"
  lcd   :       "Debug_Lcd"
  xb    :       "FullDuplexSerial"

DAT

lr_time long  0
ud_time long  0

lr      long  0
ud      long  0

lr_min  long  $7FFF_FFFF
lr_max  long  0

ud_min  long  $7FFF_FFFF
ud_max  long  0

l_motor long  0
r_motor long  0

turbo   byte  0

blanks  byte  "  ",0



pub go

  'make our turbo button an input
  dira[SW_CN]~

  'start the wireless serial communication system
  xb.start(7, 6, 0, 9600)

  'start the LCD
  lcd.init(23, 9_600, 2)

  'init the LCD
  lcd.backlight(1)
  lcd.cls
  
  'Main program loop!
  repeat
  
    lcd.home
    'get information from the analog joystick module
    rc.RCTIME(LR_PIN, 1, @lr_time)
    rc.RCTIME(UD_PIN, 1, @ud_time)

    'do auto-calibration logic
    if lr_time > lr_max
      lr_max := lr_time

    if lr_time < lr_min
      lr_min := lr_time

    if ud_time > ud_max
      ud_max := ud_time

    if ud_time < ud_min
      ud_min := ud_time

    'do some math to get the x/y position from the analog joystick (takes calibration values into account)
    lr := (100 * (lr_time-lr_min))/(lr_max - lr_min)
    ud := (100 * (ud_time-ud_min))/(ud_max - ud_min)

    lr := ||(100 - lr) - 50
    ud := ||(100 - ud)

    'do some math to translate from x/y position to left/right motor speeds
    l_motor := 0 #> ((ud*((-50 #> lr <# 0)<<1 + 100)) / 100) <# 100 
    r_motor := 0 #> ((ud*(100 - (0 #> lr <# 50)<<1) ) / 100) <# 100

    'record the turbo button state
    turbo := !(ina[SW_CN] > 0)

    'transmit start packet
    xb.tx("!")
    xb.tx("Y")
    xb.tx("B")

    'transmit our calced motor values and turbo state
    xb.tx(byte[@l_motor][0])
    xb.tx(byte[@r_motor][0])
    xb.tx(turbo)


    'print to the controller's LCD    
    lcd.putc("L")
    lcd.dec(l_motor)
    lcd.str(@blanks)
    lcd.putc("R")
    lcd.dec(r_motor)
    lcd.str(@blanks)

    lcd.putc(13)
    
    if turbo  
      lcd.str(STRING("TURBO!!!!"))
    else
      lcd.str(STRING("         "))
      
    

    
  